#include "blueTeeth.h"
#include "dirCar.h"
#include "SR04.h"
#include "pid.h"
#define LED_PIN 7
#define LED_BREATH_PIN 3
#define BEEP_PIN 3

#define T1 262
#define T2 294
#define T3 330
#define T4 349
#define T5 392
#define T6 440
#define T7 494

int music_tone[] = {
  T1, T2, T3, T1, T1, T2, T3, T1,
  T3, T4, T5, T3, T4, T5,
  T5, T6, T5, T4, T3, T1,
  T5, T6, T5, T4, T3, T1,
  T1, T5, T1, T1, T5, T1
};
int tone_time[] = {
  4, 4, 4, 4, 4, 4, 4, 4,
  4, 4, 8, 4, 4, 8,
  2, 1, 2, 1, 4, 4,
  2, 1, 2, 1, 4, 4,
  4, 4, 8, 4, 4, 8,
};

void led_init(int led_pin)
{
  pinMode(led_pin,OUTPUT);
}

//发光二极管较长的引脚接到5v
//发光二极管较短的引脚接到GPIO引脚（7号引脚）
void led_on(int led_pin)
{
  digitalWrite(led_pin,LOW);
}

void led_off(int led_pin)
{
  digitalWrite(led_pin,HIGH);
}
void led_breath(int led_pin)
{
  int i=0;
  while(1)
  {
    for(i=0;i<=255;i++)
    {
      analogWrite(led_pin,i);
      delay(5);
    }
    for(i=256;i>=0;i--)
    {
      analogWrite(led_pin,i);
      delay(5);
    }
    }
  }
  void beep_init(int beep_pin)
  {
    pinMode(beep_pin,OUTPUT);
    }
   void beep_on(int beep_pin)
    {
      tone(beep_pin,500);
      delay(1000);
      noTone(beep_pin);
      }
void play_music()
{
  int i = 0;
  for (i = 0; i<= sizeof(music_tone)/sizeof(int);i++)
  {
    tone(BEEP_PIN, music_tone[i]);
    delay(50 * tone_time[i]);
    noTone(BEEP_PIN);
  }
}
void car_follow()
{if(SR04_distance()<10)
{car_backward();}
else if(SR04_distance()>20)
{car_forward();}
else{car_stop();}
  }
void setup() {
  // put your setup code here, to run once:
  // Serial 用于硬件串口通信的对象
  // begin 这个成员函数用于初始化串口
  Serial.begin(9600);
  Serial.println("hello world");
  blueTeeth_init();
  engine_init();
  SR04_init();
  pid_init();
  
  //led_init(LED_PIN);
  //beep_init(BEEP_PIN);
  //beep_on(BEEP_PIN); 
//car_forward();
  //play_music();
  //led_breath(LED_BREATH_PIN);
}
void  loop()
{
   // put your main code here, to run repeatedly:
  //Serial.println("lajishan");
  int bytes=0;
  char ch=0;
//int distance=SR04_distance();
//car_follow();
pid_test();
  //bytes=Serial.available();
    bytes=blueTeeth.available();
    if(bytes>0)
    {
      ch=blueTeeth.read();
    if(-1==ch)
    {
      Serial.println("read fail");
      }
      else
      {
        Serial.println(ch);
        car_ctr1(ch);
    //led_on(LED_PIN);
    //delay(200);
    //led_off(LED_PIN);
        }
    }
}
